#pragma once

#include "LidarDriverX.h"

#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <rs_driver/api/lidar_driver.hpp>
#include <rs_driver/msg/pcl_point_cloud_msg.hpp>

class LidarMultiDriverX
{
public:
    LidarMultiDriverX(int driver_count, std::vector<LidarConfigurationOptions *> options);
    ~LidarMultiDriverX();

    void start();
    void stop();

    /// @brief 添加回调--多雷达数据接收
    /// @param callback
    bool addMultiPointCloudMsgCallback(PointCloudDataCallback callback);

private:
    int _driver_max_count = 4;
    int _driver_count = 0;
    std::vector<PointCloudDataCallback> _pointCloudDataCallbacks;
    LidarDriverX *_drivers[4] = {nullptr};
    LidarConfigurationOptions *_options[4] = {nullptr};

    int _que_max_size = 8;
    robosense::lidar::SyncQueue<std::shared_ptr<PointCloudData<pcl::PointXYZI>>> _stuffed_cloud_queue[4];

    bool processData(PointCloudData<pcl::PointXYZI> &data);

    bool _to_exit_process = true;
    Poco::Thread _threadPointCloudProcess;
    static void threadPointCloudProcess(void *pContext);
    void processPointCloud();
};